00100		TITLE SERVO
00200		.INSERT ASMBL.FAI[CMS,LCS]
00300	
00400	;I/O address definitions.
00500	   DAC ← 100000	;8 bit DAC.
00600	   JCR ← 120000	;Joint control output register.
00700	   ENCL ← 140000	;Encoder mux low.
00800	   ENCH ← 140001	;Encoder mux high.
00900	
01000	   STKSIZ ← 377	;Stack size.
01100	   OFF ← 377	;?
01150	   LSBENB ← 40	;Enable LSB servo.
01175	   DBLMOD ← 20	;Double all position commands for
01187		;joints that have extremely low gearing.
01200	
01300	;Zero page variables.
01400	;Not shared?
01500	
01550	IOCTRL:	0	;Copy of JCR output port.
01600	CURVEL:	BLOCK 2	;Current velocity.
01700		0
01800	SETPT:	BLOCK 3	;Current setpoint.
01900		0
02000	SETINC:	BLOCK 3	;Interpolating increment for setpoints.
02100	PREDCT:	BLOCK 3	;Result of the predictive term.
02200	LSTINX:	BLOCK 3	;Position at last index pulse.
02300	OLDSP:	BLOCK 3	;Last commanded setpoint, for CMDVEL.
02400	POSERR:	BLOCK 3	;Current position error.
02500	DACSIG:	BLOCK 3	;Scratch.
02600	
03700	BGLOCK:	0	;Interlock around background pre. cal.
03800	DSPAT:	BLOCK 2	;Dispatch address when cmds are rcvd?
03900	DSPAT2:	BLOCK 2	;Dispatch when commands are executed?
04000	INCTR:	0	;Count the interpolations.
04100	HSTTMR:	0	;Count ticks between host commands.
04200	
04300	LOGTMP:	BLOCK 4	;Temp for the arithmetic routines.
04400	CVSAV:	BLOCK 2	;Save area for background variables.
04500	VELSAV:	BLOCK 2
04600	BGTMP:	BLOCK 2
04700	
04800	ZAPEND ← .-1	;Clear all the above in startup.
04900	
05000	CURPOS:	BLOCK 3	;Current position, extended to 3 bytes.
05050	
05100	TL:	0	;Scratch for grey to binary.
05200	TH:	0
     

00100	;Shared ram. Check all refs.
00200	   LOC 200	;Second half of zero page.
00300	
00400	STATUS:	0	;Flags for the host.
00500	MODE:	0	;Mode bits from host.
00550	
00575	CKWORD:	BLOCK 2	;Host I/O check/command word.
00587	CMDPOS:	BLOCK 2	;Commanded position from host.
00600	
00620	BUSLO:	0	;Buffer?
00640	BUSHI:	0	;?
00660	
00700	MEMPTR:	BLOCK 2	;Address pointer for diagnostic read.
00720	;NINTER = function of INTSCL?
00800	NINTER:	0	;# of interpolations between position
00900			;commands.
01000	INTSCL:	0	;# of bits to shift setpoint dif for
01100			;interpolating.
01200	HSTLIM:	0	;# of clock ticks allowed between host
01300		0	;commands.
01400	CMDVEL:	BLOCK 2	;Commanded velocity.
01500	MASS:	BLOCK 2	;Inertia term for prediction.
01600	FRICTN:	BLOCK 2	;Viscous damping coefficient.
01700	GRAVTY:	BLOCK 4	;DC offset for gravity.
01800	POSTOL:	BLOCK 4	;Half-width of position tolerance band.
01900	INTTOL:	BLOCK 4	;Half-width of integration band.
     

00100	;Add SEI and CLI to all shared ram refs.
00150	;Add ? for IOCTRL read?
00200	START:	CLD
00300		LDXI	STKSIZ	;Setup stack.
00400		TXS
00500	
00520	;Zero shared ram?
00600		LDAI	0
00700		LDXI	ZAPEND
00800	RLOOP:	STAZX	0	;Reset ram.
00900		DEX
01000		BPL	RLOOP
01100		STAZ	CURPOS+2;?
01200	
01300		TAY
01400		BEQ	RSTDEF	;Jump
01500	
01600	
01700	DLOOP:	INY
01800		LDAY	INITBL	;Init ram.
01900		STAZX	0
02000		INY
02100	
02200	RSTDEF:	LDXZY	INITBL
02300		CPXI	377
02400		BNE	DLOOP
02500	
02600		JSR	POSUPD	;?
02700		JSR	SETPOS	;?
02800	
02900		CLI	;?
03000	
03100	RSTCKW:	LDAI	0	;Reset check word.
03120		SEI
03200		STAZ	CKWORD
03300		STAZ	CKWORD+1
03400		CLI
03450	;Idle loop. Wait for command.
03500	IDLE:	LDXZ	CKWORD+1;+1 for no lock.
03600		BEQ	IDLE
03700	
03800		SEI
03900		LDAZ	CKWORD	;Get check word.
04000		LDXZ	CKWORD+1
04100		CLI
04200	
04210	;CKWORD is command?
04220		SEI
04230		LDAZ	CMDPOS	;Get position.
04240		LDXZ	CMDPOS+1
04250		CLI
04260	;Position command.
04300	
04400	INITBL:	STATUS	↔	200
04500		NINTER	↔	=32
04600		INTSCL	↔	5
04700		HSTLIM	↔	=48
04800		DSPAT+1	↔	(IMBLK⊗-10)∧377
04900		DSPAT2+1   ↔	(DFBLK⊗-10)∧377
05000		DAC	↔	0
05100		377
     

00100	;Clock tick interrupt.
00200	TICK:	PHA	;Save state.
00300		TXA
00400		PHA
00500		TYA
00600		PHA
00700	
00800		LDY	ENCL	;Read encoder.
00900		LDA	ENCH
01000	
01100	;Convert from grey to binary.
01200		STAZ	TH
01300		LSRA
01400		EORZ	TH
01500		STAZ	TH
01600		TAX
01700	
01800		TYA
01900		STAZ	TL
02000		RORA
02100		EORZ	TL
02200		STAZ	TL
02300	
02400		LSRZ	TH
02500		RORA
02600		LSRZ	TH
02700		RORA
02800	
02900		EORZ	TL
03000		STAZ	TL
03100		TAY
03200		TXA
03300		EORZ	TH
03400		STAZ	TH
03500	
03600		LSRA
03700		RORZ	TL
03800		LSRA
03900		RORZ	TL
04000		LSRA
04100		RORZ	TL
04200		LSRA
04300		RORZ	TL
04400	
04500		EORZ	TH
04600		STAZ	TH
04700		TYA
04800		EORZ	TL
04900		EORZ	TH
05000		STAZ	TL	;?
05100		TAY	;?
05200	;Extend sign from n bits?
05300	;	LDXZ	TH	?
     

00100		JSR	POSUPD	;Put POSUPD here?
00200	
00300		STAZ	CURPOS
00400		STXZ	CURPOS+1
00500		STYZ	CURPOS+2
00600	
00700		DECZ	HSTTMR
00800		BPL	HOSTOK
00900	
01000		LDAI	0
01100		STAZ	HSTTMR
01200		STAZ	CMDVEL
01300		STAZ	CMDVEL+1
01400	
01450	;IOCTRL is copy of JCR?
01500	HOSTOK:	LDAI	4
01600		BITZ	IOCTRL	;?If position mode is off,
01700		BNE	INTRS
01800		JMP	CURSRV	;don't servo.
01900	
02000	;Interpolate the setpoints.
02100	INTRS:	CLC
02200		LDAZ	SETPT-1
02300		ADCZ	SETINC-1
02400		STAZ	SETPT-1
02500		LDAZ	SETPT
02600		ADCZ	SETINC
02700		STAZ	SETPT
02800		LDAZ	SETPT+1
02900		ADCZ	SETINC+1
03000		STAZ	SETPT+1
03100		LDAZ	SETPT+2
03200		ADCZ	SETINC+2
03300		STAZ	SETPT+2
03400	
03500		DECZ	INCTR
03600		BNE	GPOSER
03700	
03800		LDAI	0
03900		STAZ	SETINC-1
04000		STAZ	SETINC
04100		STAZ	SETINC+1
04200		STAZ	SETINC+2
04300	
04400	;Calculate the position error.
04500	GPOSER:	SEC
04600		LDAZ	CURPOS
04700		SBCZ	SETPT
04800		STAZ	POSERR
04900		LDAZ	CURPOS+1
05000		SBCZ	SETPT+1
05100		STAZ	POSERR+1
05200		LDAZ	CURPOS+2
05300		SBCZ	SETPT+2
05400		STAZ	POSERR+2
     

00100		BITZ	MODE	;?If servo is disabled, we're
00200		BPL	OOTOL	;automatically out of tolerance
00300	
00400		LDAZ	POSERR+2;Test the sign of pos error.
00500		BMI	NEGPER
00600	
00700		LDAZ	POSTOL	;Positive. Compare with tol.
00800		CMPZ	POSERR
00900		LDAZ	POSTOL+1
01000		SBCZ	POSERR+1
01100		LDAI	0
01200		SBCZ	POSERR+2
01300		BCS	TOLOK	;In tolerance.
01400		BCC	OOTOL	;Jump.
01500	
01600	NEGPER:	CLC	;Negative. Add the tolerance.
01700		LDAZ	POSTOL
01800		ADCZ	POSERR
01900		LDAZ	POSTOL+1
02000		ADCZ	POSERR+1
02100		LDAI	0
02200		ADCZ	POSERR+2
02300		BCS	TOLOK	;In tolerance.
02400	
02500	OOTOL:	LDAZ	IOCTRL	;Out of tolerance.
02600		ANDI	177	;Turn off the in tolerance
02700		BNE	WCNTRL	;indicator.
02800	
02900	TOLOK:	LDAZ	IOCTRL	;In tolerance. Turn it on.
03000		ORAI	200
03100	WCNTRL:	STAZ	IOCTRL
03150		STA	JCR	;Copy it to output.
03200	
03300		BITZ	MODE	;If intergration is disabled,
03400		BVC	OOBAND	;turn it off.
03500		LDAZ	POSERR+2;Test sign of position error.
03600		BMI	ADTOL
03700	
03800		LDAZ	INTTOL	;Positive. Compare with tol.
03900		CMPZ	POSERR
04000		LDAZ	INTTOL+1
04100		SBCZ	POSERR+1
04200		LDAI	0
04300		SBCZ	POSERR+2
04400		BCS	INBAND
04500		BCC	OOBAND
04600	
04700	ADTOL:	CLC	;Negative. Add the tolerance.
04800		LDAZ	INTTOL
04900		ADCZ	POSERR
05000		LDAZ	INTTOL+1
05100		ADCZ	POSERR+1
05200		LDAI	0
05300		ADCZ	POSERR+2
05400		BCS	INBAND
05500	
05600	OOBAND:	LDAZ	IOCTRL	;Out of band. Turn off
05700		ORAI	10	;integration by setting the
05800		ANDI	357	;control bit. LSB servo off.
05900		BNE	WCTRL2
     

00100	INBAND:	LDAI	LSBENB	;In band. Is LSB servo enabled
00200		BITZ	MODE
00300		BEQ	RCNTRL
00400	
00500		LDAZ	POSERR	;Yes. Is the error exactly 0?
00600		ORAZ	POSERR+1
00700		ORAZ	POSERR+2
00800		BNE	RCNTRL
00900	
01000		LDAZ	IOCTRL	;It is. Integration off, LSB
01100		ORAI	30	;servo on.
01200		BNE	WCTRL2	;Jump.
01300	
01400	RCNTRL:	LDAZ	IOCTRL	;LSB disabled or error
01500		ANDI	347	;not zero. LSB servo off,
01600				;integration on.
01700	
01800	WCTRL2:	STAZ	IOCTRL
01900		STA	JCR	;Output it.
     

00100		LDAZ	LOGTMP	;Since the arithmetic routines
00200		LDYZ	LOGTMP+1;aren't re-entrant, we need to
00300		STAZ	LOGTMP+2;save their state here.
00400		STYZ	LOGTMP+3
00500	
00600		LDYZ	CURVEL	;Get the velocity,
00700		LDAZ	CURVEL+1
00800		JSR	LOG
00900		LDXI	FRICTN	;mult. by the friction
01000		JSR	MULTIP	;coefficient,
01100		JSR	EXP
01200		TAX
01300		TYA
01400		CLC	;add the position error...
01500		ADCZ	POSERR
01600		STAZ	DACSIG
01700		TXA
01800		ADCZ	POSERR+1
01900		STAZ	DACSIG+1
02000		LDYI	0
02100		TXA	;(sign-extend the velocity)
02200		BPL	NODEY
02300		DEY
02400	
02500	NODEY:	TYA
02600		ADCZ	POSERR+2
02700		STAZ	DACSIG+2
02800	
02900		CLC	;...the velocity predictive term...
03000		LDAZ	DACSIG
03100		ADCZ	PREDCT
03200		STAZ	DACSIG
03300		LDAZ	DACSIG+1
03400		ADCZ	PREDCT+1
03500		STAZ	DACSIG+1
03600		LDAZ	DACSIG+2
03700		ADCZ	PREDCT+2
03800		STAZ	DACSIG+2
03900	
04000		CLC	;...and the gracity offset.
04100		LDAZ	DACSIG
04200		ADCZ	GRAVTY
04300		TAY
04400		LDAZ	DACSIG+1
04500		ADCZ	GRAVTY+1
04600		TAX
04700		LDAZ	DACSIG+2
04800		ADCZ	GRAVTY+2
04900	
04950	;Put PUTDAC here?
05000		JSR	PUTDAC	;Put result out to the DAC.
05100	
05200		LDYZ	LOGTMP+3;Restore the arithmetic
05300		LDAZ	LOGTMP+2;routines' state.
05400		STYZ	LOGTMP+1
05500		STAZ	LOGTMP
05600	CMDSP:		;Add deferred commands here?
     

00050	;Change CMDEND for no host command interrupt?
00100	CMDEND:	LDAI	4	;Done with commands.
00200		BITZ	IOCTRL	;Are we servoing?
00300		BEQ	INTXIT
00400		BITZ	BGLOCK	;Yes. Is the background
00500		BMI	INTXIT	;predictor still running?
00600	
00700		DECZ	BGLOCK	;No. Start it up.
00800		JMP	BGSRV
00900	
01000	BGDON:	INCZ	BGLOCK	;Unlock?
01100	
01200	INTXIT:	PLA	;Restore state and dismiss interrupt.
01300		TAY
01400		PLA
01500		TAX
01600		PLA
01700		RTI
01800	
01850	;Stop mode?
01900	CURSRV:		;Not servoing ("Current mode")...
02000		JMP	CMDSP
02100	
02200	;Background velocity prediction.
02300	BGSRV:	LDAZ	CURVEL	;Copy the variables used to
02400		STAZ	VELSAV	;avoid interference from
02500		LDAZ	CURVEL+1;interrupts while this routine
02600		STAZ	VELSAV+1;is running.
02700		LDAZ	CMDVEL
02800		STAZ	CVSAV
02900		LDAZ	CMDVEL+1
03000		STAZ	CVSAV+1
03100		LDYZ	POSERR
03200		LDAZ	POSERR+1
03300		LDXZ	POSERR+2
03400	
03500		CLI	;Enable interrupts?
03600	
03700		PHA
03800		ASLA	;Is magnitude of position error
03900		TXA	;< 2↑15?
04000		ADCI	0
04100		BEQ	FLOERR
04200	
04300		PLA	;No. Set the predictive term to zero.
04400		LDAI	0
04500		TAX
04600		TAY
04700		JMP	NTRLOC
04800	
04900	FLOERR:	PLA	;Yes. Float the position error.
05000		JSR	LOG
05100		JSR	INV	;TMP = 1 / POSERR
05200		STYZ	BGTMP
05300		STAZ	BGTMP+1
05400		CLC
05500		LDAZ	CVSAV	;Commanded velocity + current
05600		ADCZ	VELSAV	;velocity...
05700		TAY
05800		LDAZ	CVSAV+1
05900		ADCZ	VELSAV+1
     

00100		JSR	LOG	;...float...
00200		LDXI	BGTMP
00300		JSR	MULTIP	;...* TMP...
00400		STYZ	BGTMP	;...stored at TMP.
00500		STAZ	BGTMP+1
00600		SEC
00700		LDAZ	CVSAV	;Commanded velocity - current
00800		SBCZ	VELSAV	;velocity...
00900		TAY
01000		LDAZ	CVSAV+1
01100		SBCZ	VELSAV+1
01200		JSR	LOG	;...same thing.
01300		LDXI	BGTMP
01400		JSR	MULTIP
01500		STYZ	BGTMP
01600		STAZ	BGTMP+1
01700	
01800		SEI	;Interlock...
01900	
02000		LDYZ	MASS	;...get the mass...
02100		LDAZ	MASS+1
02200	
02300		CLI	;clear the lock.
02400	
02500		JSR	MULTIP	;Scale the predictor.
02600		JSR	EXP	;Back to integer form.
02700		LDXI	0
02800		CMPI	0
02900		BPL	NTRLOC	;Extend sign to 3 bytes.
03000		DEX
03100	
03200	NTRLOC:	SEI	;End of background. Interlock.
03300	
03400		STYZ	PREDCT
03500		STAZ	PREDCT+1;Store the result for the servo
03600		STXZ	PREDCT+2;to use.
03700		JMP	BGDON
     

00100	INTBL:		;IMMEDIATE COMMAND TABLE?
00200		HCIRDM∧377	;Read memory.
00300	
00400		HCISRV∧377	;Position mode?
00500	
00600	CMTBL:		;DEFERRED COMMAND TABLE?
00900		CMDEND∧377	;Read memory?
01000	
01100		CMDSRV∧377	;Position mode?
     

00100	;Subroutines?
00200	;Enter with position in A (low), X (middle), Y (high).
00300	;Sets current position to that value, puts the setpoint
00400	;to the same, clears the setpoint interpolating
00500	;increment, and goes into stop mode.
00600	;??
01400	SETPOS:	STAZ	CURPOS	;Set the current position.
01500		STXZ	CURPOS+1
01600		STYZ	CURPOS+2
01700	
01800	;Second entry - freeze to the position in A, X, Y as
01900	;above without changing the current position.
02000	;??
02100	FREZE:	STAZ	SETPT	;Set the position command.
02200		STXZ	SETPT+1
02300		STYZ	SETPT+2
02400		STAZ	OLDSP
02500		STXZ	OLDSP+1
02600		STYZ	OLDSP+2
02700	
02800		LDAI	75	;I/O control bits for servo
02900		STAZ	IOCTRL	;enable on, all others off.
02950		STA	JCR
03000	
03100		LDAI	0
03200		STAZ	SETPT-1	;Clear the setpoint extension
03300		STAZ	SETINC-1;and the interpolator
03400		STAZ	SETINC
03500		STAZ	SETINC+1
03600		STAZ	SETINC+2
03700		STAZ	CMDVEL	;and the commanded velocity.
03800		STAZ	CMDVEL+1
03900	
04000		LDAZ	SETPT	;Return the regs. unchanged.
04100		RTS
04200	
04300	;Enter with low counter value in Y.
04400	;Returns updated position in A (low), X (middle),
04500	;Y (high). Also sets CURVEL to the 16-bit signed
04600	;velocity.
04700	;??
04800	POSUPD:	STAZ	DACSIG+1;Save high byte.
04820		TYA
05100		STAZ	DACSIG	;Save that value.
05300		SEC
05400		SBCZ	CURPOS	;Subtract the old position
05500		STAZ	CURVEL	;yielding the velocity.
05520		LDAZ	DACSIG+1
05540		SBCZ	CURPOS+1
05560		STAZ	CURVEL+1
05900		LDXZ	CURPOS+1	;Set up for updating bytes
06000		LDYZ	CURPOS+2	;2 and 3.
06100		LDAZ	DACSIG+1;Did bit 15 of pos. change?
06200		EORZ	CURPOS+1
06300		BPL	GETDAC	;If not, we're through.
06400		LDAZ	CURVEL+1;It did. Which way did we move
06500		BMI	DOWN
06600		LDAZ	DACSIG+1;Upward.
06700		BMI	GETDAC	;If bit 15 is on, we're done.
07000		INY	;Off. Increment high byte.
07100		JMP	GETDAC
07200	
07300	DOWN:	LDAZ	DACSIG+1;Downward.
07400		BPL	GETDAC	;If bit 15 is off, we're done.
07500		DEY	;Decrement high byte.
07900	
08000	GETDAC:	LDAZ	DACSIG
08100		RTS
     

00100	;DAC output subroutine. Not sub?
00200	;Enter with 3 byte value in Y (low), X (middle),
00300	;A (high). Clobbers all registers, but the 8 bits the
00400	;DAC got are returned in?
00500	PUTDAC:	BMI	NEGDAC	;Assuming the last I. loaded A.
00600		CPYI	200	;Positive. Compare with 2↑7.
00700		BCS	TOOHI
00800		CPXI	1
00900		SBCI	0
01000		BCC	INRNGE
01100	
01200	TOOHI:	LDYI	177	;Too high. Saturate positive.
01300		BNE	INRNGE	;Jump.
01400	
01500	NEGDAC:	CPYI	200	;Negative. Compare with -2↑7.
01600		BCC	TOOLOW
01700	
01800		CPXI	377
01900		SBCI	377
02000		BCS	INRNGE
02100	
02200	TOOLOW:	LDYI	200	;Too low. Saturate to -2↑7.
02300	
02400	INRNGE:	STY	DAC	;Output 8 bits to the DAC.
02500		RTS
02600	
02700	DOUBLE:	PHA	;Doubles the position in (Y,X,A) if
02800		LDAI	DBLMOD	;the double mode bit is set.
02900		BITZ	MODE
03000		BEQ	NOTDBL
03100		PLA
03200		ASLA
03300		PHA
03400		TXA
03500		ROLA
03600		TAX
03700		TYA
03800		ROLA
03900		TAY
04000	NOTDBL:	PLA
04100		RTS
04200	
04300	HALVE:	PHA	;Halve the position argument in (Y,X,A)
04400		LDAI	DBLMOD	;if the double mode bit is set.
04500		BITZ	MODE
04600		BEQ	NOTDBL
04700		TYA
04800		CMPI	200
04900		RORA
05000		TAY
05100		TXA
05200		RORA
05300		TAX
05400		PLA
05500		RORA
05600		RTS
     

00100	;No index?
00200	ENBTST:	PHA	;Test for servo enabled.
00300		LDAZ	MODE
00400		ANDI	202
00500		CMPI	200
00600		BNE	NOTENB
00700		PLA	;OK. Return.
00720		RTS
00800	
00900	NOTENB:	PLA	;No. Wipe the return address and
01000		PLA	;end this command.
01100		PLA
01200		JMP	CMDEND
     

00100	;Arithmetic routines.
00200	;Enter with high byte in A, low in Y.
00300	;Returns A = characteristic and sign, Y = mantissa.
00400	;Clobbers X, LOGTMP, LOGTMP+1.
00500	LOG:	STYZ	LOGTMP	;Save the inputs.
00600		STAZ	LOGTMP+1
00700	
00800		LDXI	20+100	;?Init characteristic to 15.
00900		CMPI	0	;Test sign of input.
01000		BPL	POSIN
01100		SEC	;Negative. 2's complement it.
01200		LDAI	0
01300		SBCZ	LOGTMP
01400		STAZ	LOGTMP
01500		LDAI	0
01600		SBCZ	LOGTMP+1
01700	POSIN:	BNE	NORML	;Is high byte zero?
01800		LDAZ	LOGTMP	;Yes. Low byte?
01900		BEQ	RTRN	;If so, return zero.
02000		LDYI	0	;Low nonzero. Shift left one
02100		STYZ	LOGTMP	;byte,
02200		LDXI	10+100	;?change characteristic to 7.
02300	NORML:	DEX	;Normalize the number, counting the
02400		ASLZ	LOGTMP	;characteristic down. When the
02500		ROLA	;first "1" shifts out, we've subtracted
02600		BCC	NORML	;1 from the normalized number
02700		ASLZ	LOGTMP	;(This rounds the result)
02800		ADCI	=11	;and are left with the fraction
02900		TAY	;Adding 11 to that is equivalent to
03000		TXA	;adding 0.043.
03100		ADCI	0	;Propagate the carry into the
03200				;characteristic.
03300		ASLA	;Insert the sign bit from the saved
03400		ASLZ	LOGTMP+1;input.
03500		RORA
03600	RTRN:	RTS	;Done.
03700	
03800	;Enter with sign and characteristic in A, mantissa in Y
03900	;Returns 16-bit integer, low byte in Y, high in A.
04000	;Clobbers X, LOGTMP, LOGTMP+1.
04100	EXP:	STAZ	LOGTMP+1;Save sign of input.
04200		ANDI	177	;Mask it off.
04300		BEQ	ZEROIN	;Zero characteristic returns
04400		TAX	;zero.
04500		TYA	;Get the mantissa...
04600		SEC
04700		SBCI	=11	;...subtract 0.043...
04800		STAZ	LOGTMP	;(save this value)
04900		TXA	;...propagate the carry and get rid
05000		SBCI	100	;of the XS-64 offset.
05100		BMI	NEGIN	;If negative (value < 1.0)
05200				;return zero.
05300		CMPI	=15	;Test for overflow (value>=2↑15
05400		BCS	SATUR
05500		TAX	;...no. Number is in range.
05600		ADCI	-10	;?Is characteristic below 8?
05700		BMI	BLOATE
05800		TAX	;No. Reduce if by 8,
05900		JSR	UNNORM	;unnormalize.
06000		BMI	GETTMP	;Jump.
     

00100	BLOATE:	JSR	UNNORM	;Yes. Unnormalize, then
00200		ASLZ	LOGTMP	;(round result)
00300		ADCI	0
00400		STAZ	LOGTMP	;use result as low byte and
00500		LDAI	0	;set high byte to zero.
00600	
00700	GETTMP:	LDYZ	LOGTMP
00800	GTMP1:	LDXZ	LOGTMP+1;Test sign of input...
00900		BPL	POSIGN
01000		STAZ	LOGTMP+1;...negative. 2's complement
01100		LDAI	0	;the result.
01200		SEC
01300		SBCZ	LOGTMP
01400		TAY
01500		LDAI	0
01600		SBCZ	LOGTMP+1
01700	POSIGN:	RTS
01800	
01900	NEGIN:	LDAI	0	;Set the result to zero if the
02000	ZEROIN:	TAY	;input is negative.
02100		RTS
02200	
02300	SATUR:	LDYI	OFF	;Saturate result to 2↑15 - 1 if
02400		STYZ	LOGTMP	;input was 15 or more.
02500		LDAI	177
02600		BNE	GTMP1	;Jump.
02700	
02800	UNNORM:	LDAI	1	;Unnormalize subroutine. Add 1
02900		BNE	DECRX	;to the fraction.
03000	
03100	SCALE:	ASLZ	LOGTMP	;Scale the fraction left by the
03200		ROLA	;amount of the characteristic.
03300	DECRX:	DEX
03400		BPL	SCALE
03500		RTS
03600	
03700	;Enter with characteristic of multiplier in A,
03800	;mantissa in Y, X pointing to a pair of base page
03900	;locations containing the multiplicand (mantissa in the
04000	;low byte).
04100	;Returns the product in A and Y, same form as the
04200	;multiplier. Leaves X unchanged. Clobbers LOGTMP and
04300	;LOGTMP+1.
04400	MULTIP:	PHA
04500		EORZX	1	;Compute sign of result,
04600		STAZ	LOGTMP+1	;save it away.
04700		PLA
04800		ANDI	177	;Mask off multiplier sign.
04900		BEQ	ZEROIN	;If zero, return zero.
05000		STAZ	LOGTMP
05100		TYA	;Add the two logarithms.
05200		CLC
05300		ADCZX	0
05400		TAY
05500		LDAZX	1
05600		ANDI	177	;If multiplicand is zero,
05700		BEQ	ZEROIN	;return a zero.
05800		ADCZ	LOGTMP
05900		SEC
06000		SBCI	100	;Correct the XS-64 offset.
     

00100		BPL	INSIGN	;Result in range?
00200		ANDI	100	;No. If underflow,
00300		BNE	NEGIN	;return zero.
00400		LDAI	177	;Overflow. Saturate to
00500		LDYI	377	;highest magnitude.
00600	
00700	INSIGN:	ASLA	;Insert the sign of the result.
00800		ASLZ	LOGTMP+1
00900		RORA
01000		RTS
01100	
01200	;Inverse function: 2's complement the magnitude part
01300	;of a 15-bit logarithm.
01400	;Enter with characteristic in A, mantissa in Y.
01500	;Returns inverse in the same form. X unchanged.
01600	;Clobbers LOGTMP and LOGTMP+1.
01700	INV:	STYZ	LOGTMP	;Pretty straightforward...
01800		STAZ	LOGTMP+1
01900		SEC
02000		LDAI	0	;Complement the number by
02100		SBCZ	LOGTMP	;subtracting it from zero.
02200		TAY
02300		LDAI	0
02400		SBCZ	LOGTMP+1
02500		JMP	INSIGN	;Insert the original sign.
     

00100	;DEFERRED COMMANDS.
00200	;Fix GRAVTY+2, POSTOL+2, and INTTOL+2.
00300	;Add DOUBLE to POSTOL and INTTOL.
00400	
00420	   LOC (.∨377)+1	;For start of next page.
00500	DFBLK ← .
00600	;Set parameter command?
00700	CMDSET:	STAZX	0	;?
00800		JMP	CMDEND
05200	
05300	;CMDCUR:	Stop mode?
     

00100	CMDSRV:	JSR	ENBTST
00200		JSR	DOUBLE
00300		STAZ	DACSIG
00400		STXZ	DACSIG+1
00500		STYZ	DACSIG+2
00600	
00700		SEC
00800		SBCZ	SETPT
00900		STAZ	SETINC
01000		TXA
01100		SBCZ	SETPT+1
01200		STAZ	SETINC+1
01300		TYA
01400		SBCZ	SETPT+2
01500		LDXI	0
01600		STXZ	SETPT-1
01700		STXZ	SETINC-1
01800		LDXZ	INTSCL
01900	
02000	SCAL:	CMPI	200	;Extend sign.
02100		RORA
02200		RORZ	SETINC+1
02300		RORZ	SETINC
02400		RORZ	SETINC-1
02500		DEX
02600		BNE	SCAL
02700	
02800		STAZ	SETINC+2
02900		LDAZ	NINTER
03000		STAZ	INCTR
03100		SEC
03200		LDAZ	DACSIG
03300		SBCZ	OLDSP
03400		STAZ	CMDVEL
03500		LDAZ	DACSIG+1
03600		SBCZ	OLDSP+1
03700		STAZ	CMDVEL+1
03800		LDAZ	DACSIG
03900		STAZ	OLDSP
04000		LDAZ	DACSIG+1
04100		STAZ	OLDSP+1
04200		LDAZ	DACSIG+2
04300		STAZ	OLDSP+2
04400	
04500		LDAZ	IOCTRL
04600		ORAI	44
04700		STAZ	IOCTRL
04750		STA	JCR	;Output it.
04800	
04900		LDAZ	HSTLIM	;Reset host timer.
05000		STAZ	HSTTMR
05100		JMP	CMDEND
     

00100	;Immediate commands.
00200	
00250	   LOC (.∨377)+1	;For start of next page.
00300	IMBLK ← .
00400	HCISRV:		;?
00500	
02100	;Sync, ack?
02200	HCIRDM:	LDYZ	MEMPTR
02300		LDAY	0
02400		LDXY	1
02500		INY
02600		INY
02700		STYZ	MEMPTR
02800	
02900		STAZ	BUSLO	;?
03000		STXZ	BUSHI
03100		JMP	INTXIT
03200	
03300	;Add HALVE to CURPOS. Fix CURPOS+2.
03400	
03600	HCINOP:		;?
03700	   ;Ack host.
03800		JMP	INTXIT
03900	
04000	   NMI ← START	;Reset??
04100	;Interrupt vectors.
04200	   LOC 177772
04300		NMI∧377
04400		(NMI⊗-10)∧377
04500		START∧377
04600		(START⊗-10)∧377
04700		TICK∧377
04800		(TICK⊗-10)∧377
04900	END